from machine import Pin,I2C,ADC
from servo import Servos
import utime
import icm20948
import math
i2c=I2C(1, sda=Pin(0), scl=Pin(1),freq=400000)
imu = icm20948.ICM20948(I2C(0),dmp=True)
utime.sleep(0.5)
arm= Servos(i2c,address=0x40)
utime.sleep(0.5)
catch=ADC(Pin(7))
catch.equ(ADC.EQU_MODEL_2)
utime.sleep(0.1)

arm.position(0,90)
utime.sleep(0.1)
arm.position(2,70)
utime.sleep(0.1)
arm.position(3,149)#@
utime.sleep(0.1)
arm.position(6,35)#@
utime.sleep(0.1)
arm.position(8,90)
utime.sleep(0.1)
arm.position(10,140)
utime.sleep(0.1)
def Quaternion():
    q1=0
    q2=0
    q3=0
    q0=0
    if imu.dataupdate() and imu.dataready():
        quat_6= imu.quat6()
        q1 = quat_6[0]/math.pow(2, 30)
        q2 = quat_6[1]/math.pow(2, 30)
        q3 = quat_6[2]/math.pow(2, 30)
        q0 = math.sqrt(abs(1-(q1*q1+q2*q2+q3*q3)))
    while not(imu.dataupdate() and imu.dataready()):
        quat_6= imu.quat6()
        q1 = quat_6[0]/math.pow(2, 30)
        q2 = quat_6[1]/math.pow(2, 30)
        q3 = quat_6[2]/math.pow(2, 30)
        q0 = math.sqrt(abs(1-(q1*q1+q2*q2+q3*q3)))
    return [q0,q1,q2,q3]

def to_euler_angles(data):
    Thre=2*(data[0]*data[2]-data[3]*data[1])
    while(Thre>=1 or Thre<=-1):
        data=Quaternion()
        Thre=2*(data[0]*data[2]-data[3]*data[1])
    eu_w=data[0]
    eu_x=data[1]
    eu_y=data[2]
    eu_z=data[3]
    angles = [0,0,0]
    eu_r = math.atan2(2*(eu_w*eu_x+eu_y*eu_z),1-2*(eu_x*eu_x+eu_y*eu_y))
    cfg=0
    while(cfg==0):
        try:
            eu_p = math.asin(2*(eu_w*eu_y-eu_z*eu_x))
        except ValueError as e:
            print(2*(eu_w*eu_y-eu_z*eu_x))
            print("EV")
        else:
            cfg=1
    eu_e = math.atan2(2*(eu_w*eu_z+eu_x*eu_y),1-2*(eu_z*eu_z+eu_y*eu_y))
    angles[0] = eu_r*180/math.pi#+180
    angles[1] = eu_p*180/math.pi#+180
    angles[2] = eu_e*180/math.pi#+180
    return angles

shift=to_euler_angles(Quaternion())

def get_ang():
    try:
        sign=0
        imu_data=Quaternion()
        imu_data=Quaternion()
        #imu_data=Quaternion()
        msg=to_euler_angles(imu_data)
    except ValueError as e:
        sign=0
    except TypeError as e:
        sign=0
    except OSError as e:
        sign=0
    else:
        sign=1
    if(sign==1):
        msg[0]=int((msg[0]-shift[0]+90))
        msg[1]=int((msg[1]-shift[1]+90))
        msg[2]=int((msg[2]-shift[2]+90))
        
        msg[2]=abs(msg[2])
        msg[0]=abs(msg[0])
    return msg

def movement(num,ang):
    if ang<=180 and ang>=0 :
        arm.position(num,ang)

clamp_position=140



get_ang()
get_ang()
delta_ang=[0,0,0]
pre_ang=get_ang()
now_ang=get_ang()
delta_ang[0]=now_ang[0]-pre_ang[0]
delta_ang[2]=now_ang[2]-pre_ang[2]
arm_angy=80+delta_ang[0]
arm_angz=90+delta_ang[2]
a=1
i=0

while True:
    i+=1
    if i%31==0 and a<=11 :
        a+=1
    pre_ang[0]=arm_angy
    pre_ang[2]=arm_angz
    
    now_ang=get_ang()
    now_ang=get_ang()
    
    delta_ang[0]=now_ang[0]-pre_ang[0]+a
    delta_ang[2]=now_ang[2]-pre_ang[2]+a
    movement(0,arm_angz)
    movement(2,arm_angy)
    adc=catch.read()
    if clamp_position >=120 and clamp_position <=180:
        if adc>=1800:
            clamp_position-=5
            arm.position(10,clamp_position)
        elif adc<=135:
            clamp_position+=5
            arm.position(10,clamp_position)
    elif clamp_position<120:
        clamp_position=120
    elif clamp_position >180:
        clamp_position=180
    arm_angy+=delta_ang[0]
    utime.sleep_us(10)
    arm_angz+=delta_ang[2]
    # print("y  z")
    # print(arm_angy)
    # print(arm_angz)
    

        